#pragma once

#include "smatrix.h"
#include <math/cubic_quartic.h>
#include <limits>

namespace g2 
{

/// finds the angle phi, such that it gives minimum to the expression
/// (x * Ax + 2 b * x), where x = (cos(phi), sin(phi))
template <typename T>
T optimal_angle(const basic_smatrix<T> & A, const basic_vector<T> & b, basic_vector<T> * o_x = 0)
{
  // eigenvalues of A
  T l1, l2; 
  // eigenvectors of A
  basic_vector<T> om1 = A.eigenvector(0, true, &l1);
  basic_vector<T> om2 = A.eigenvector(1, true, &l2);

  T l11 = l1 * l1;
  T l12 = l1 * l2;
  T l22 = l2 * l2;

  T b1 = scalar_product(om1, b);
  T b2 = scalar_product(om2, b);
  T b11 = scalar_product(b1, b1);
  T b22 = scalar_product(b2, b2);
  //b1 + b2 == b.lengthSqr

  T root[4];
  int numRoots = quartic(
    -2 * (l1 + l2), 
    (l11 + l22 + 4*l12 - (b11 + b22)), 
    2 * (l1 * (b22 - l22) + l2 * (b11 - l11)), 
    l11 * l22 - b11 * l22 - b22 * l11,
    root);
  if (numRoots <= 0)
  {
    assert (false);
    return 0;
  }

  basic_vector<T> x;
  T xValue = std::numeric_limits<T>::max();
  for (int i = 0; i < numRoots; ++i)
  {
    T tmu = root[i];
    basic_vector<T> y(b1 / (tmu - l1), b2 / (tmu - l2));
    basic_vector<T> tx( //= Om * y
      om1.x * y.x + om2.x * y.y,
      om1.y * y.x + om2.y * y.y);
    T tValue = tmu + scalar_product(b, tx);
    if (tValue < xValue)
    {
      x = tx;
      xValue = tValue;
    }
  }

  T phi = atan2(x.y, x.x);

//   //T check
//   T min = x * A(x) + 2 * (b * x);
//   for (T psi = 0; psi < 3.15; psi += 0.01)
//   {
//     basic_vector<T> t(cos(psi), sin(psi));
//     T v = t * A(t) + 2 * (b * t);
//     assert (v >= min);
//   }

  if (o_x)
	  *o_x = x;
  return phi;
}

} //namespace g2 
